using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace 九点标定
{
    public class NPointCalibration
    {
        /// <summary>
        /// 创建标定矩阵
        /// </summary>
        /// <param name="px">原始点X</param>
        /// <param name="py">原始点Y</param>
        /// <param name="qx">目标点位X</param>
        /// <param name="qy">目标点位Y</param>
        /// <param name="matrix">标定矩阵</param>
        /// <returns>执行结果</returns>
        public bool CreateCalibMatrix(double[] px, double[] py, double[] qx, double[] qy, out double[] matrix)
        {
            matrix = null;

            try
            {
                //校验参数
                if ((px.Length != py.Length) || (px.Length != qx.Length) || (qx.Length != qy.Length))
                {
                    return false;
                }

                int ParamLength = px.Length;

                double[,] p = new double[4, 4];
                double[] q = new double[3];
                double[] q2 = new double[3];
                double[,] A = new double[4, 4];
                double[,] A2 = new double[4, 4];
                double[,] B = new double[2, 4];
                double[,] B2 = new double[2, 4];
                double[,] C = new double[1, 4];
                double[,] C2 = new double[1, 4];

                for (int i = 0; i < ParamLength; i++)
                {
                    p[0, 0] += px[i] * px[i];
                    p[0, 1] += px[i] * py[i];
                    p[0, 2] += px[i];
                    p[1, 0] = p[0, 1];
                    p[1, 1] += py[i] * py[i];
                    p[1, 2] += py[i];
                    p[2, 0] = p[0, 2];
                    p[2, 1] = p[1, 2];
                    p[2, 2] += 1;
                }

                for (int i = 0; i < ParamLength; i++)
                {
                    q[0] += -px[i] * qx[i];
                    q[1] += -py[i] * qx[i];
                    q[2] += -qx[i];
                    q2[0] += -px[i] * qy[i];
                    q2[1] += -py[i] * qy[i];
                    q2[2] += -qy[i];
                }

                for (int i = 0; i < 3; i++)
                {
                    A[i, 0] = p[i, 0];
                    A[i, 1] = p[i, 1];
                    A[i, 2] = p[i, 2];
                    A[i, 3] = q[i];

                    A2[i, 0] = p[i, 0];
                    A2[i, 1] = p[i, 1];
                    A2[i, 2] = p[i, 2];
                    A2[i, 3] = q2[i];
                }

                double m2 = Math.Abs(A[1, 2]);
                double b = A[0, 2] * A[1, 2] > 0 ? -1 : 1;
                double m3 = b * Math.Abs(A[0, 2]);
                for (int j = 0; j < 4; j++)
                {
                    B[0, j] = A[0, j] * m2 + A[1, j] * m3;
                }

                double m0 = Math.Abs(A[2, 2]);
                double k = A[0, 2] * A[2, 2] > 0 ? -1 : 1;
                double mi = k * Math.Abs(A[0, 2]);

                for (int j = 0; j < 4; j++)
                {
                    B[1, j] = A[0, j] * m0 + A[2, j] * mi;
                }

                double m5 = Math.Abs(B[1, 1]);
                double t = B[0, 1] * B[1, 1] > 0 ? -1 : 1;
                double m1 = t * Math.Abs(B[0, 1]);
                for (int j = 0; j < 4; j++)
                {
                    C[0, j] = B[0, j] * m5 + B[1, j] * m1;
                }

                //the factor of axis x
                double[] tRe = new double[6];
                tRe[0] = (0 - C[0, 3]) / C[0, 0];
                tRe[1] = (0 - B[0, 3] - B[0, 0] * tRe[0]) / B[0, 1];
                tRe[2] = (0 - A[0, 3] - A[0, 0] * tRe[0] - A[0, 1] * tRe[1]) / A[0, 2];

                //get axis y
                double n2 = Math.Abs(A2[1, 2]);
                double b22 = A2[0, 2] * A2[1, 2] > 0 ? -1 : 1;
                double n3 = b22 * Math.Abs(A2[0, 2]);
                for (int j = 0; j < 4; j++)
                {
                    B2[0, j] = A2[0, j] * n2 + A2[1, j] * m3;
                }

                double n0 = Math.Abs(A2[2, 2]);
                double k2 = A2[0, 2] * A2[2, 2] > 0 ? -1 : 1;
                double ni = k2 * Math.Abs(A2[0, 2]);
                for (int j = 0; j < 4; j++)
                {
                    B2[1, j] = A2[0, j] * n0 + A2[2, j] * ni;
                }


                double n5 = Math.Abs(B2[1, 1]);
                double t2 = B[0, 1] * B2[1, 1] > 0 ? -1 : 1;
                double n1 = t * Math.Abs(B2[0, 1]);
                for (int j = 0; j < 4; j++)
                {
                    C2[0, j] = B2[0, j] * n5 + B2[1, j] * n1;
                }

                //the factor of axis y
                tRe[3] = (0 - C2[0, 3]) / C2[0, 0];
                tRe[4] = (0 - B2[0, 3] - B2[0, 0] * tRe[3]) / B2[0, 1];
                tRe[5] = (0 - A2[0, 3] - A2[0, 0] * tRe[3] - A2[0, 1] * tRe[4]) / A2[0, 2];

                matrix = tRe;

                foreach (var item in tRe)
                {
                    if (double.IsNaN(item))
                    {
                        return false;
                    }
                }

                return true;
            }
            catch (Exception)
            {

            }

            return false;
        }


        /// <summary>
        /// 标定
        /// </summary>
        /// <param name="matrix">标定矩阵</param>
        /// <param name="px">原始点X</param>
        /// <param name="py">原始点Y</param>
        /// <param name="qx">目标点位X</param>
        /// <param name="qy">目标点位Y</param>
        /// <returns>执行结果</returns>
        public bool Calibrate(double[] matrix, double px, double py, out double qx, out double qy)
        {
            qx = -1;
            qy = -1;

            try
            {
                double[] QxArray;
                double[] QyArray;

                if (Calibrate(matrix, new double[1] { px }, new double[1] { py }, out QxArray, out QyArray))
                {
                    qx = QxArray[0];
                    qy = QyArray[0];

                    return true;
                }

            }
            catch (Exception)
            {

            }

            return false;
        }

    }
}
